package com.htfyun.serialport.uartservo.AX12_Servo;

import com.htfyun.serialport.uartservo.IServoCommand;
import com.htfyun.serialport.utils.SerialDebugLog;

import java.nio.ByteBuffer;

public class AX12_ServoCommand extends IServoCommand {

    @Override
    public byte[] getHeader() {
        return AX12_ReceivedSerialPacket.getHeader();
    }

    //CheckSum = ~(ID + Length + Instruction + Parameter1 + ... + Parameter N)
    @Override
    public byte[] getCheckSum() {
        byte[] id = getId();
        byte[] parameters = getParameters();
        byte[] instruction = getInstruction();

        byte[] retByte = new byte[0];
        if (id == null) {
            SerialDebugLog.e(" error id is null");
            return retByte;
        }

        if (parameters == null) {
            parameters = new byte[0];
        }

        if (instruction == null) {
            SerialDebugLog.e(" error instruction is null");
            return retByte;
        }

        byte length = (byte) (parameters.length + 2); //指令包的长度，该值表示为“参数个数(N)+2”

        if (id.length != 1) {
            SerialDebugLog.e("error id length(" + id.length + "), AX12 id length should be 1!");
            return retByte;
        }

        byte realID = id[0];

        if (instruction.length != 1) {
            SerialDebugLog.e("error instruction length(" + instruction.length + "), AX12 instruction length should be 1!");
            return retByte;
        }

        byte realInstruction = instruction[0];

        int checkSumInt = (realID + length + realInstruction);
        for (byte para : parameters) {
            checkSumInt += (int) (para & 0xff);
        }

        checkSumInt = ~checkSumInt;

        return new byte[]{(byte) checkSumInt};
    }

    //header,id,len,instruction,parameters,checksum
    @Override
    public byte[] buildForCommand() {
        byte[] header = getHeader();
        byte[] id = getId();
        byte[] parameters = getParameters();
        byte[] instruction = getInstruction();
        byte[] checkSum = getCheckSum();

        byte[] retByte = new byte[0];

        if (header == null) {
            SerialDebugLog.e(" error header is null");
            return retByte;
        }

        if (id == null) {
            SerialDebugLog.e(" error id is null");
            return retByte;
        }

        if (parameters == null) {
            parameters = new byte[0];
        }

        if (instruction == null) {
            SerialDebugLog.e(" error instruction is null");
            return retByte;
        }

        if (checkSum == null) {
            SerialDebugLog.e(" error checkSum is null");
            return retByte;
        }

        byte length = (byte) (parameters.length + 2); //指令包的长度，该值表示为“参数个数(N)+2”

        int commandLen = header.length + id.length + 1/*len*/ + instruction.length + parameters.length + checkSum.length;
        ByteBuffer byteBuffer = ByteBuffer.allocate(commandLen);
        byteBuffer.put(header);
        byteBuffer.put(id);
        byteBuffer.put(length);
        byteBuffer.put(instruction);
        byteBuffer.put(parameters);
        byteBuffer.put(checkSum);

        return byteBuffer.array();
    }

}
